#include "DJ_Motor_Driver.h"
#include <stdlib.h>
#include "PID.h"
#include "BSP.h"

static uint8_t Motor_Count = 0; // 电机数量
static DJ_meature_t *Motors;    // 电机参数结构体数组指针

// CAN中断回调函数
void DJ_CAN_Callback(CAN_RxHeaderTypeDef *pHeader, uint8_t *pBuf)
{
    switch (pHeader->StdId)
    {
    case DJ_M1_ID:
    case DJ_M2_ID:
    case DJ_M3_ID:
    case DJ_M4_ID:
    {
        static uint8_t i = 0;

        i = pHeader->StdId - DJ_M1_ID;

        Motors[i].last_angle = Motors[i].angle;
        Motors[i].angle = (uint16_t)(pBuf[0] << 8 | pBuf[1]);
        Motors[i].speed = (uint16_t)(pBuf[2] << 8 | pBuf[3]);
        Motors[i].current = (uint16_t)(pBuf[4] << 8 | pBuf[5]);
        Motors[i].temperature = pBuf[6];

        if (Motors[i].angle - Motors[i].last_angle > 4096)
        {
            Motors[i].round_count--;
        }
        else if (Motors[i].angle - Motors[i].last_angle < -4096)
        {
            Motors[i].round_count++;
        }
        Motors[i].total_angle = (Motors[i].round_count + (Motors[i].angle - Motors[i].offset_angle) / 8192.0f) * 360.0f / Motors[i].decratio;
    }
    }
}

void DJ_Init(uint8_t Motor_ID, uint8_t Motor_Type)
{
    Motors = (DJ_meature_t *)realloc(Motors, (Motor_Count + 1) * sizeof(DJ_meature_t));

    if (Motor_Type == M2006)
    {
        Motors[Motor_Count].decratio = DJ_M2006_DECRATIO;
        // 速度PID初始化
        PID_Init(&Motors[Motor_Count].PID_Speed, 5.0f, 0.5f, 0.0f, 16000.0f, 1000.0f);

        // 角度PID初始化
        PID_Init(&Motors[Motor_Count].PID_Angle, 150.0f, 0.5f, 10.0f, 25000.0f, 100.0f);
        PID_Init(&Motors[Motor_Count].PID_SpeedOfAngle, 5.0f, 0.0f, 0.0f, 16000.0f, 500.0f);
    }
    else
    {
        Motors[Motor_Count].decratio = DJ_M3508_DECRATIO;
        // 速度PID初始化
        PID_Init(&Motors[Motor_Count].PID_Speed, 5.0f, 0.5f, 0.0f, 16000.0f, 1000.0f);

        // 角度PID初始化
        PID_Init(&Motors[Motor_Count].PID_Angle, 150.0f, 0.5f, 10.0f, 8000.0f, 30.0f);
        PID_Init(&Motors[Motor_Count].PID_SpeedOfAngle, 5.0f, 0.0f, 0.0f, 16000.0f, 500.0f);
    }

    Motors[Motor_Count].ID = Motor_ID;
    Motors[Motor_Count].motor_type = Motor_Type;
    Motor_Count++;
}

void DJ_SetCurrent(int16_t motor1_current, int16_t motor2_current, int16_t motor3_current, int16_t motor4_current)
{
    uint8_t data[8];

    data[0] = motor1_current >> 8;
    data[1] = motor1_current;
    data[2] = motor2_current >> 8;
    data[3] = motor2_current;
    data[4] = motor3_current >> 8;
    data[5] = motor3_current;
    data[6] = motor4_current >> 8;
    data[7] = motor4_current;

    CAN_Transmit(&DJ_MOTOR_CAN, DJ_ALL_ID, data);
}

/**
 * 设置电机速度
 * 逆时针转为正
 */
void DJ_SetSpeed(int16_t *set_speed_array)
{
    for (uint8_t i = 0; i < 4; i++)
    {
        PID_Calc(&Motors[i].PID_Speed, (float)(DJ_GetMeasure(i + 1)->speed), (float)set_speed_array[i]);
    }

    DJ_SetCurrent((int16_t)Motors[0].PID_Speed.out, (int16_t)Motors[1].PID_Speed.out, (int16_t)Motors[2].PID_Speed.out, (int16_t)Motors[3].PID_Speed.out);
}

void DJ_SetAngle(int16_t *set_angle_array)
{
    for (uint8_t i = 0; i < 4; i++)
    {
        // 串级PID控制
        PID_Calc(&Motors[i].PID_Angle, (float)(DJ_GetMeasure(i + 1)->total_angle), (float)set_angle_array[i]);
        PID_Calc(&Motors[i].PID_SpeedOfAngle, (float)(DJ_GetMeasure(i + 1)->speed), Motors[i].PID_Angle.out);
    }

    DJ_SetCurrent((int16_t)Motors[0].PID_SpeedOfAngle.out, (int16_t)Motors[1].PID_SpeedOfAngle.out, (int16_t)Motors[2].PID_SpeedOfAngle.out, (int16_t)Motors[3].PID_SpeedOfAngle.out);
}

DJ_meature_t *DJ_GetMeasure(uint8_t ID)
{
    return &Motors[ID - 1];
}

void DJ_GetOffset(void)
{
    for (uint8_t i = 0; i < 4; i++)
    {
        Motors[i].offset_angle = Motors[i].last_angle;
    }
}

void DJ_ClearAngle(void)
{
    for (uint8_t i = 0; i < 4; i++)
    {
        Motors[i].total_angle = 0;
        Motors[i].round_count = 0;
    }
}
